/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    state_StandUp.cpp
  * @author  Mentos Seetoo
  * @brief   Finite State Machine for standing up.
  * @note    
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
#include "state_Jump.h"
/* Private defines -----------------------------------------------------------*/
using namespace Quadruped;

/* Founctions ----------------------------------------------------------------*/
template <typename T>
FSM_Jump<T>::FSM_Jump(GaitScheduler<T> *p_gait,
                    StateEstimator<T> *p_estimator,
                    DesiredCoMState<T> *p_command,
                    Dynamic<T> *p_dynamic,
                    BalanceController<T> *p_controller,
                    T run_period):FSM_Base<T>::FSM_Base(run_period)
{
    FSM_Base<T>::_name = FSM_StateName::JUMP;
    p_BalanceController = p_controller;
    p_Dynamic = p_dynamic;
    p_GaitScheduler = p_gait;
    p_CoMCommand = p_command;
    p_StateEstimator = p_estimator;
}


template <typename T>
void FSM_Jump<T>::RunState()
{
        for (int leg(0); leg < 4; leg++)
        {
            //!>ready to jump
            if (p_GaitScheduler->GetEndBeforeJumpFlag()!=1)
            {
                pStandPos[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0, 0,-p_CoMCommand->CoM_height) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);   
                    pBeforeJumpPos[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(-0.05, 0,-p_CoMCommand->CoM_beforeJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);        
                
                p_GaitScheduler->SetFootPlacement(pStandPos[leg], pBeforeJumpPos[leg], 0.00, leg);  
            }
            //!>execute jump
            else if(p_GaitScheduler->GetEndBeforeJumpFlag()==1 && p_GaitScheduler->GetEndBoundJumpFlag()!=1)
            {
                    pBoundJump[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(-0.3, 0,-p_CoMCommand->CoM_boundJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);
                p_GaitScheduler->SetFootPlacement(pBeforeJumpPos[leg],pBoundJump[leg],0.0,leg);
            }
            //!>retract leg.
            else if(p_GaitScheduler->GetEndBoundJumpFlag()==1 && p_GaitScheduler->GetEndRetractFlag()!=1)
            {
                    pRetractPos[leg]=p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(-0.05, 0,-0.15) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);     
                    p_GaitScheduler->SetFootPlacement(pRetractPos[leg],pRetractPos[leg],0.0,leg);
            }
            //!>ready to fall down.
            else if (p_GaitScheduler->GetEndRetractFlag()==1)
            {
                /*This part is temporary no use.*/
                if(once_use)
                {
                    once_use--;
                    pFallPos[leg]=p_StateEstimator->p_foot_M[leg]; 
                }
                pBeforeJumpPos[leg] = p_Dynamic->BodyState.rBody_M2B *
                    (Vec3<T>(0.03, 0,-p_CoMCommand->CoM_beforeJumpHeight) + p_Dynamic->getHipLocation(leg) + getLegSign(leg) * p_Dynamic->_hipLocation);
                p_GaitScheduler->SetFootPlacement(pFallPos[leg],pBeforeJumpPos[leg],0.0,leg);
            }
        }   
    //!< Generate trajectory in {M}.
    p_GaitScheduler->gaitGenerate();

    for (int leg(0); leg < 4; leg++)
    {
        //!> set targets in {B}.
        // if (p_GaitScheduler->GetEndRetractFlag()==1)
        // {
            
        //     resistance = 0.1;

        //     if ( p_StateEstimator->p_foot_M[leg](2)<=pFallPos[leg](2)-0.01)
        //     {
        //         pDesiredFoot[leg] +=  resistance*(p_StateEstimator->p_foot_M[leg]-pFallPos[leg]);
        //     }
        //     else if (p_StateEstimator->p_foot_M[leg](2)>pFallPos[leg](2)+0.01)
        //     {
        //         pDesiredFoot[leg] -=  resistance*(p_StateEstimator->p_foot_M[leg]-pFallPos[leg]);
        //     }
        //     else
        //     {
        //         pDesiredFoot[leg] = p_GaitScheduler->GetFootPos(leg);
        //     }
        // }
        // else
        // {
        //     pDesiredFoot[leg] = p_GaitScheduler->GetFootPos(leg);
        // }
        pDesiredFoot[leg] = p_GaitScheduler->GetFootPos(leg);

        p_Dynamic->LegCommand[leg].p = (p_Dynamic->BodyState.rBody_M2B.transpose()
                                     * pDesiredFoot[leg]) - p_Dynamic->getHipLocation(leg);
        
        SysLog->Record(_INFO_,"Scheduler","Leg Command in M-Frame %d: %f, %f, %f",leg,
                        pDesiredFoot[leg](0),
                        pDesiredFoot[leg](1), 
                        pDesiredFoot[leg](2));
        SysLog->Record(_INFO_,"Scheduler","Leg Command in B-Frame %d: %f, %f, %f",leg,
                                    p_Dynamic->LegCommand[leg].p(0), p_Dynamic->LegCommand[leg].p(1), p_Dynamic->LegCommand[leg].p(2));
    }
}

/**
* @brief  
*         
* @return 0: transition done.
*         1: 
*         2: error transition request.
* @author Mentos Seetoo
*/
template <typename T>
int FSM_Jump<T>::Transition(FSM_Base<T>* target_state)
{
    //Ready to switch
    switch (target_state->StateName())
    {
        case FSM_StateName::PASSIVE:
            p_GaitScheduler->changeGait(GaitType::STAND_UP);
            SysLog->Record(_INFO_,"FSM","State is switched to PASSIVE...");
            return 0;
        case FSM_StateName::LOCOMOTION:
            return 2;
        default:
            SysLog->Record(_ERROR_,"FSM","Error transition in STAND_UP, request is ignored...");
            return 2;
    }
}


template class FSM_Jump<float>;
template class FSM_Jump<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
